#include <serialStick.hpp>
#include <iostream>
#include <thread>
#include <motor_CAN.hpp>

using namespace std;

int main(int argc, char **argv)
{
    cout << "serial stick driver test" << endl;

    SerialStick stick;
    if (!stick.InitDevice("/dev/ttyS6", 115200))
    {
        cout << "open Stick Serial fail" << endl;
        return -1;
    }
    
    auto server = make_shared<CanServer>();
    if (!server->StartServer("can0"))
    {
        cout << "Open Can Server fail." << endl;
        return -1;
    }

    Motor_Can motorL;
    MotorConfig cfgL{.address = 0x70, .poleCount = 15};
    if (!motorL.InitDevice(server, cfgL))
    {
        cout << "Init motor fail" << endl;
        return -1;
    }
    Motor_Can motorR;
    MotorConfig cfgR{.address = 0x1c, .poleCount = 15};
    if (!motorR.InitDevice(server, cfgR))
    {
        cout << "Init motor fail" << endl;
        return -1;
    }

    while (true)
    {
        stick.ReadData();
        for (int i = 0; i < 4; i++)
        {
            float value = stick.GetAix(i);
            std::cout << value << '\t';
        }
        motorL.SendHeartBeat();
        motorL.SetCurrent(stick.GetAix(0)*2000);
        motorR.SendHeartBeat();
        motorR.SetCurrent(stick.GetAix(0)*2000);

        std::cout << std::endl;
        this_thread::sleep_for(chrono::milliseconds(10));
    }
    return 0;
}